/* Private includes ----------------------------------------------------------*/
#include "hfi_q.h"
#include "data.h"
#include "fliter.h"
#include "foc.h"
#include "user_lib.h"
/* Private define ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
HFI hfi;
float kp = 200.0f;
float ki = 20000.0f;
/* Private function prototypes -----------------------------------------------*/
/*
1.极性辨识
2.20k采样 10k电流环 10k注入
    ClarkeTrans(&foc);
    HFI_AlphaBetah_Get(&hfi, foc.i_alpha, foc.i_beta);
    HFI_Pos_err(&hfi);
    PLL_Cale_HFI(&hfi);
    foc.theta = hfi.Angle;
    SinCosCalculate(&foc);
    ParkTrans(&foc);
    if(hfi.hfi_init_flag == 0)
    {
      NS_Check(&hfi);
    }
    HFI_dqf_Get(&hfi, foc.i_d, foc.i_q);
    Curr_PI_Cal_VoltCircLimit(&Iqd_PI, target_iq, target_id, hfi.i_qf, hfi.i_df);
    // Curr_PI_Cal_VoltCircLimit(&Iqd_PI, 1.5f, 0.0f, foc.i_q, foc.i_d);
    foc.u_d = HFI_Injc_Volt(&hfi, Iqd_PI.id_output);
    // foc.u_d = Iqd_PI.id_output;
    foc.u_q = Iqd_PI.iq_output;
    IparkTrans(&foc);
    SVPWM(&foc);
*/

void HFI_Init(HFI *hfi)
{
  hfi->u_in_flag = 0;
  hfi->u_in      = 1.0f;
  hfi->Ts        = 0.000025f;

  hfi->sign = 0;

  hfi->Kp = kp;
  hfi->Ki = ki;
}

float HFI_Injc_Volt(HFI *hfi, float ud_in)
{
  float a;
  switch(hfi->u_in_flag)
  {
    case 0:
    {
      a              = ud_in + hfi->u_in;
      hfi->u_in_flag = 1;
      break;
    }
    case 1:
    {
      a              = ud_in - hfi->u_in;
      hfi->u_in_flag = 0;
      break;
    }
  }
  return a;
}

void HFI_AlphaBetah_Get(HFI *hfi, float i_alpha, float i_beta)
{
  hfi->i_alphas = i_alpha;
  hfi->i_betas  = i_beta;

  hfi->i_alphah = (hfi->i_alphas - 2 * hfi->i_alphas_last + hfi->i_alphas_last_last) / 4.0f;
  hfi->i_betah  = (hfi->i_betas - 2 * hfi->i_betas_last + hfi->i_betas_last_last) / 4.0f;

  hfi->i_alphas_last_last = hfi->i_alphas_last;
  hfi->i_betas_last_last  = hfi->i_betas_last;
  hfi->i_alphas_last      = hfi->i_alphas;
  hfi->i_betas_last       = hfi->i_betas;
}

void HFI_dqf_Get(HFI *hfi, float i_ds, float i_qs)
{
  hfi->i_ds = i_ds;
  hfi->i_qs = i_qs;
  hfi->i_df = (hfi->i_ds + 2 * hfi->i_ds_last + hfi->i_ds_last_last) / 4.0f;
  hfi->i_qf = (hfi->i_qs + 2 * hfi->i_qs_last + hfi->i_qs_last_last) / 4.0f;

  hfi->i_dh = (hfi->i_ds - 2 * hfi->i_ds_last + hfi->i_ds_last_last) / 4.0f;
  hfi->i_qh = (hfi->i_qs - 2 * hfi->i_qs_last + hfi->i_qs_last_last) / 4.0f;

  hfi->i_ds_last_last = hfi->i_ds_last;
  hfi->i_qs_last_last = hfi->i_qs_last;
  hfi->i_ds_last      = hfi->i_ds;
  hfi->i_qs_last      = hfi->i_qs;
}

void HFI_Pos_err(HFI *hfi)
{
  if(hfi->u_in_flag == 1)
    hfi->sign = 1;
  else if(hfi->u_in_flag == 0)
    hfi->sign = -1;

  hfi->i_alphah_delta = (hfi->i_alphah - hfi->i_alphah_last);
  hfi->i_betah_delta  = (hfi->i_betah - hfi->i_betah_last);
  hfi->ia             = (hfi->i_alphah_delta) * hfi->sign;
  hfi->ib             = (hfi->i_betah_delta) * hfi->sign;

  hfi->i_alphah_last = hfi->i_alphah;
  hfi->i_betah_last  = hfi->i_betah;
}

float PLL_LPF(float data)
{
  static float alpha = 0.1f;
  static float y, Last_y;
  y      = alpha * data + Last_y * (1 - alpha);
  Last_y = y;
  return y;
}

void PLL_Cale_HFI(HFI *hfi)
{
  hfi->Angle_last = hfi->Angle;
  hfi->Angle      = Limit_Angle(hfi->Angle);
  hfi->sin_pll    = arm_sin_f32(hfi->Angle);
  hfi->cos_pll    = arm_cos_f32(hfi->Angle);

  hfi->Angle_Err  = -hfi->ia * hfi->sin_pll + hfi->ib * hfi->cos_pll;
  hfi->P          = hfi->Kp * hfi->Angle_Err;
  hfi->I         += hfi->Ki * hfi->Angle_Err;

  hfi->I = Limit(hfi->I, 5000.0f);

  hfi->Omega = hfi->P + hfi->I;

  hfi->Angle += hfi->Omega * hfi->Ts;
  hfi->Omega  = PLL_LPF(hfi->Omega);
}

void NS_Check(HFI *hfi)
{
  /*获取高频响应电流*/
  hfi->Angle_delta = hfi->Angle - hfi->Angle_last;
  if(hfi->Angle_delta < 0) hfi->Angle_delta = _2_PI + hfi->Angle - hfi->Angle_last;
  if(hfi->Angle_delta < 0.2f)
  {
    if(hfi->ns_check_cnt < 100)
    {
      hfi->trg_iq = 0.0f;
      hfi->trg_id = 0.0f;
    }
    else if(hfi->ns_check_cnt == 100)
    {
      hfi->trg_id = 2.0f;
    }
    else if(hfi->ns_check_cnt >= 1000 && hfi->ns_check_cnt <= 3000)  // 等待磁路稳定采样
    {
      hfi->trg_id  = 2.0f;
      hfi->sum1   += fabs(hfi->i_dh * 100.0f);
    }
    else if(hfi->ns_check_cnt >= 3100 && hfi->ns_check_cnt <= 6000)  // 不充磁
    {
      hfi->trg_id = 0.0f;
    }
    else if(hfi->ns_check_cnt == 6100)
    {
      hfi->trg_id = -2.0f;
    }
    else if(hfi->ns_check_cnt >= 7000 && hfi->ns_check_cnt <= 9000)
    {
      hfi->trg_id  = -2.0f;
      hfi->sum2   += fabs(hfi->i_dh * 100.0f);
    }
    else if(hfi->ns_check_cnt > 9100)
    {
      hfi->trg_id = 0.0f;
      if(hfi->sum1 > hfi->sum2)
      {
        hfi->Angle = hfi->Angle + 3.1416f;
        if(hfi->Angle > _2_PI) hfi->Angle -= _2_PI;
      }
      hfi->trg_iq        = 1.0f;
      hfi->hfi_init_flag = 1;
    }
  }
  hfi->ns_check_cnt++;
}
